/**
 * @file vtk_utils.h
 * @copyright Copyright (c) 2019, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef NOETHER_VTK_UTILS_H
#define NOETHER_VTK_UTILS_H

#include <pcl/common/common.h>
#include <pcl/point_traits.h>
#include <pcl/point_types.h>
#include <pcl/PolygonMesh.h>

#include <vtkSmartPointer.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkCutter.h>

namespace vtk_viewer
{
/**
 * @brief Enum used to define the type of plane generated by createPlane
 */
enum plane_type
{
  FLAT = 0,
  SINUSOIDAL_1D = 1,
  SINUSOIDAL_2D = 2
};

/**
 * @brief createPlane Creates a surface mesh
 * @param gridSize number of points used to create each dimension of the grid. The total number of points is gridSize^2
 * @param type Defines the type of surface to be created
 *
 * FLAT = flat surface, a true plane
 * SINUSOIDAL_1D = Sinusoidal in one dimension
 * SINUSOIDAL_2D = Sinusoidal in two dimensions
 * @return The points associated with the mesh
 */
vtkSmartPointer<vtkPoints> createPlane(unsigned int grid_size_x = 10,
                                       unsigned int grid_size_y = 10,
                                       vtk_viewer::plane_type = vtk_viewer::SINUSOIDAL_2D);

/**
 * @brief visualizePlane Creates a simple VTK viewer to visualize a mesh object
 * @param polydata The input mesh object to visualize
 */
void visualizePlane(vtkSmartPointer<vtkPolyData>& polydata);

/**
 * @brief createMesh Uses surface reconstruction to approximate a mesh to fit the data
 * @param points The set of input points
 * @param sample_spacing The size of the triangles to form in the final mesh
 * @param neighborhood_size The number of points to consider for determining cell size and orientation
 * @return The mesh object after triangulation
 */
vtkSmartPointer<vtkPolyData> createMesh(vtkSmartPointer<vtkPoints> points, double sample_spacing, int neigborhood_size);

/**
 * @brief cleanMesh Given a mesh, removes cells that do not have enough point data nearby (called after createMesh)
 * @param points The set of input points used to eliminate empty/extraneous cells
 * @param mesh The input mesh to filter and remove empty cells from
 */
void cleanMesh(const vtkSmartPointer<vtkPoints>& points, vtkSmartPointer<vtkPolyData>& mesh);

/**
 * @brief readSTLFile Reads an STL file and returns a VTK data object
 * @param file The input file to read
 * @return A VTK polydata object representing the CAD file, returns a null pointer if the operation failed
 */
vtkSmartPointer<vtkPolyData> readSTLFile(std::string file);

/**
 * @brief estimateCurvature Estimates the curvature of a given mesh (currently experimental)
 * @param mesh The input mesh to operate on
 * @param method The desired method for curvature estimation [0-3]
 * @return
 */
vtkSmartPointer<vtkPolyData> estimateCurvature(vtkSmartPointer<vtkPolyData> mesh, int method);

/**
 * @brief embedRightHandRuleNormals Embeds the Polydata cells with the normals generated from the right hand rule
 *
 * This is as opposed to generateNormals which takes the cell point normals, averages them, and uses that average as the
 * cell normal Note: This does assume that the mesh is formed correctly with uniformly oriented normals. If not, use
 * something like Meshlab to fix it.
 * @param data The input mesh to operate. When this function returns, the normals will be embedded.
 * @return true if successful.
 */
bool embedRightHandRuleNormals(vtkSmartPointer<vtkPolyData>& data);

/**
 * @brief generateNormals Generate point and cell (surface) normals (in place)
 * @param data The mesh to generate and add normals to
 */
void generateNormals(vtkSmartPointer<vtkPolyData>& data, int flip_normals = 1);

/**
 * @brief sampleMesh Uniformly samples point on a mesh
 * @param mesh The input mesh to sample
 * @param distance The spacing between sample points on the surface
 * @return The set of points located on the surface of the mesh
 */
vtkSmartPointer<vtkPolyData> sampleMesh(vtkSmartPointer<vtkPolyData> mesh, double distance);

/**
 * @brief loadPCDFile Load a PCL PCD file and convert it to VTK
 * @param file The input file to load
 * @param polydata [output] The VTK object to return
 * @param background Optional point cloud to be used to perform background subtraction
 * @param return_mesh [currently unused] If true, computes and returns a mesh, if false, will only return the point data
 * @return True if the file exists and was loaded, False if there was an error
 */
bool loadPCDFile(std::string file,
                 vtkSmartPointer<vtkPolyData>& polydata,
                 std::string background = "",
                 bool return_mesh = true);

void vtkSurfaceReconstructionMesh(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, vtkSmartPointer<vtkPolyData>& mesh);

/**
 * @brief PCLtoVTK Converts a PCL point cloud to VTK format
 * @param cloud The input PCL point cloud
 * @param pdata the output VTK data object
 */
void PCLtoVTK(const pcl::PointCloud<pcl::PointXYZ>& cloud, vtkPolyData* const pdata);

void VTKtoPCL(vtkPolyData* const pdata, pcl::PointCloud<pcl::PointNormal>& cloud);

/**
 * @brief removeBackground Removes points from an input cloud using a background cloud as a reference (also removes
 * NaN's)
 * @param cloud The input cloud to perform background subtraction on
 * @param background The reference background cloud
 */
void removeBackground(pcl::PointCloud<pcl::PointXYZ>& cloud, const pcl::PointCloud<pcl::PointXYZ>& background);

/**
 * @brief pt_dist Computes the sum of the square distance between two points (no sqrt() to save time)
 * @param pt1 Pointer to a double array of size 3, first point for calculating distance
 * @param pt2 Pointer to a double array of size 3, second point for calculating distance
 * @return The calculated sum squared distance
 */
double pt_dist(double* pt1, double* pt2);

/**
 * @brief cutMesh Given a list of points, will cut a section out of mesh
 * @param mesh The mesh to cut from
 * @param points The list of points defining a loop in 3D space
 * @param get_inside What to return.  If true, will return the inside section, if false, will return the outside section
 * @return The resulting mesh after cutting
 */
vtkSmartPointer<vtkPolyData> cutMesh(vtkSmartPointer<vtkPolyData>& mesh,
                                     vtkSmartPointer<vtkPoints>& points,
                                     bool get_inside);

/**
 * @brief pclEstimateNormals Wrapper around PCL's normal estimation.  Estimates normals and appends them to the input
 * cloud
 * @param cloud The input cloud without normals
 * @param radius The radius to use for the nearest neighbors search for estimating normals
 * @param view_point The point of view of the 'observer' for the purposes of determining the orientation of the normals
 * @return The output cloud with normals
 */
pcl::PointCloud<pcl::PointNormal>::Ptr pclEstimateNormals(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                                          double radius = 0.01,
                                                          const pcl::PointXYZ& view_point = pcl::PointXYZ(0, 0, 5.0));

/**
 * @brief pclGridProjectionMesh Wrapper around PCL's grid projection meshing algorithm
 * @param cloud
 * @param resolution
 * @param padding_size
 * @param max_binary_searc_level
 * @param nearest_neighbors
 * @return
 */
pcl::PolygonMesh pclGridProjectionMesh(pcl::PointCloud<pcl::PointNormal>::ConstPtr cloud,
                                       double resolution = 0.003,
                                       int padding_size = 1,
                                       int max_binary_searc_level = 6,
                                       int nearest_neighbors = 20);

/**
 * @brief pclEncodeMeshAndNormals Computes normals for a mesh and converts it to a VTK polydata type (useful for path
 * planning)
 * @param pcl_mesh The input PCL mesh object, without normals
 * @param vtk_mesh The output VTK mesh object, with normals
 * @param radius The radius to use for estimating normals
 * @param view_point The point of view of the 'observer' for the purposes of determining the orientation of the normals
 */
void pclEncodeMeshAndNormals(const pcl::PolygonMesh& pcl_mesh,
                             vtkSmartPointer<vtkPolyData>& vtk_mesh,
                             double radius = 0.01,
                             const pcl::PointXYZ& view_point = pcl::PointXYZ(0, 0, 5.0));

/**
 * @brief loadPolygonMeshFromPLY Load a pcl::PolygonMesh from a file
 * @param file The file to read from
 * @param mesh The mesh to return
 * @return True if the file exists and was loaded correctly, false if there was a failure
 */
bool loadPolygonMeshFromPLY(const std::string& file, pcl::PolygonMesh& mesh);

}  // namespace vtk_viewer
#endif  // VTK_UTILS_H
